#include <rei_ydlidar_nodelet/ydlidar_nodelet.h>
namespace rei_ydlidar
{
void ReiYdlidar::onInit(){
    publish_scan_thread_ = std::make_shared<std::thread>(std::bind(&ReiYdlidar::run, this));
    ydlidar_ = std::make_shared<CYdLidar>();
    
}

void ReiYdlidar::run(){
    ros::NodeHandle nh  = getNodeHandle();
    ros::NodeHandle& nh_private = getPrivateNodeHandle();
    scan_pub = nh_private.advertise<sensor_msgs::LaserScan>("scan", 1);
    // stop_scan_service = nh_private.advertiseService("stop_scan", ReiYdlidar::stop_scan, this);
    // start_scan_service = nh_private.advertiseService("start_scan", ReiYdlidar::start_scan, this);
    nh_private.param<std::string>("port", str_optvalue, "/dev/ydlidar");
    ///lidar port
    ydlidar_->setlidaropt(LidarPropSerialPort, str_optvalue.c_str(),
                    str_optvalue.size());

    ///ignore array
    nh_private.param<std::string>("ignore_array", str_optvalue, "");
    ydlidar_->setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(),
                    str_optvalue.size());

    nh_private.param<std::string>("frame_id", frame_id, "laser_frame");

    //////////////////////int property/////////////////
    /// lidar baudrate
    nh_private.param<int>("baudrate", optval, 230400);
    ydlidar_->setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
    /// tof lidar
    optval = TYPE_TRIANGLE;
    nh_private.param<int>("lidar_type", optval, TYPE_TRIANGLE);
    ydlidar_->setlidaropt(LidarPropLidarType, &optval, sizeof(int));
    /// device type
    optval = YDLIDAR_TYPE_SERIAL;
    nh_private.param<int>("device_type", optval, YDLIDAR_TYPE_SERIAL);
    ydlidar_->setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
    /// sample rate
    optval = 9;
    nh_private.param<int>("sample_rate", optval, 9);
    ydlidar_->setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
    /// abnormal count
    optval = 4;
    nh_private.param<int>("abnormal_check_count", optval, 4);
    ydlidar_->setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));


    //////////////////////bool property/////////////////
    /// fixed angle resolution
    nh_private.param<bool>("resolution_fixed", b_optvalue, true);
    ydlidar_->setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
    /// rotate 180
    nh_private.param<bool>("reversion", b_optvalue, true);
    ydlidar_->setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
    /// Counterclockwise
    nh_private.param<bool>("inverted", b_optvalue, true);
    ydlidar_->setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
    b_optvalue = true;
    nh_private.param<bool>("auto_reconnect", b_optvalue, true);
    ydlidar_->setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
    /// one-way communication
    b_optvalue = false;
    nh_private.param<bool>("isSingleChannel", b_optvalue, false);
    ydlidar_->setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
    /// intensity
    b_optvalue = false;
    nh_private.param<bool>("intensity", b_optvalue, false);
    ydlidar_->setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
    /// Motor DTR
    b_optvalue = false;
    nh_private.param<bool>("support_motor_dtr", b_optvalue, false);
    ydlidar_->setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));

    //////////////////////float property/////////////////
    /// unit: °
    nh_private.param<float>("angle_max", f_optvalue, 180.f);
    ydlidar_->setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
    f_optvalue = -180.0f;
    nh_private.param<float>("angle_min", f_optvalue, -180.f);
    ydlidar_->setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
    /// unit: m
    f_optvalue = 16.f;
    nh_private.param<float>("range_max", f_optvalue, 16.f);
    ydlidar_->setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
    f_optvalue = 0.1f;
    nh_private.param<float>("range_min", f_optvalue, 0.1f);
    ydlidar_->setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
    /// unit: Hz
    f_optvalue = 10.f;
    nh_private.param<float>("frequency", f_optvalue, 10.f);
    ydlidar_->setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));

    nh_private.param<bool>("invalid_range_is_inf", invalid_range_is_inf,
                            invalid_range_is_inf);


    bool ret = ydlidar_->initialize();
    if (ret) {//success
    //Start the device scanning routine which runs on a separate thread and enable motor.
        ret = ydlidar_->turnOn();
    } else {
        ROS_ERROR("%s\n", ydlidar_->DescribeError());
    }
    ros::Rate r(30);
    while(ros::ok()){
        LaserScan scan;
        if (ydlidar_->doProcessSimple(scan)) {
            sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan);
        //      ydlidar_ros_driver::LaserFan fan;
            ros::Time start_scan_time;
            start_scan_time.sec = scan.stamp / 1000000000ul;
            start_scan_time.nsec = scan.stamp % 1000000000ul;
            scan_msg->header.stamp = start_scan_time;
            scan_msg->header.frame_id = frame_id;
            scan_msg->angle_min = (scan.config.min_angle);
            scan_msg->angle_max = (scan.config.max_angle);
            scan_msg->angle_increment = (scan.config.angle_increment);
            scan_msg->scan_time = scan.config.scan_time;
            scan_msg->time_increment = scan.config.time_increment;
            scan_msg->range_min = (scan.config.min_range);
            scan_msg->range_max = (scan.config.max_range);

            int size = (scan.config.max_angle - scan.config.min_angle) /
                        scan.config.angle_increment + 1;
            scan_msg->ranges.resize(size,
                                    invalid_range_is_inf ? std::numeric_limits<float>::infinity() : 0.0);
            scan_msg->intensities.resize(size);
            int idx_intensity = 0;
            int idx_timestamp = 1;

            for (size_t i = 0; i < scan.points.size(); i++) {
                int index = std::ceil((scan.points[i].angle - scan.config.min_angle) /
                                    scan.config.angle_increment);
                if(scan.points[i].intensity == 1016) continue;

                if (index >= 0 && index < size) {
                    if (scan.points[i].range >= scan.config.min_range) {
                        scan_msg->ranges[index] = scan.points[i].range;
                        scan_msg->intensities[index] = scan.points[i].intensity;
                    }
                }
            }

            scan_pub.publish(scan_msg);
        } else {
        ROS_ERROR("Failed to get Lidar Data");
        }

        r.sleep();
    }

    
}
ReiYdlidar::~ReiYdlidar(){
    ydlidar_->turnOff();
    ROS_INFO("[YDLIDAR INFO] Now YDLIDAR is stopping .......");
    ydlidar_->disconnecting();
    publish_scan_thread_->join();
}
}
PLUGINLIB_EXPORT_CLASS(rei_ydlidar::ReiYdlidar, nodelet::Nodelet);